/*
Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
Copyright (C) 2011 Gomspace ApS (http://www.gomspace.com)
Copyright (C) 2011 AAUSAT3 Project (http://aausat3.space.aau.dk) 

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

/*
Inspired by c-pthread-queue by Matthew Dickinson
http://code.google.com/p/c-pthread-queue/
*/

#include <pthread.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>

/* CSP includes */
#include "pthread_queue.h"

pthread_queue_t * pthread_queue_create(int length, size_t item_size) {
    
    pthread_queue_t * q = malloc(sizeof(pthread_queue_t));
    
    if (q != NULL) {
        q->buffer = malloc(length*item_size);
        if (q->buffer != NULL) {
            q->size = length;
            q->item_size = item_size;
            q->items = 0;
            q->in = 0;
            q->out = 0;
            if (pthread_mutex_init(&(q->mutex), NULL) || pthread_cond_init(&(q->cond_full), NULL) || pthread_cond_init(&(q->cond_empty), NULL)) {
            	free(q->buffer);
                free(q);
                q = NULL;
            }
        } else {
            free(q);
            q = NULL;
        }
    }

    return q;
    
}

void pthread_queue_delete(pthread_queue_t * q) {

	if (q == NULL)
		return;

	free(q->buffer);
	free(q);

	return;

}
    

int pthread_queue_enqueue(pthread_queue_t * queue, void * value, int timeout) {
    
    int ret;

    /* Calculate timeout */
    struct timespec ts;
    if (clock_gettime(CLOCK_REALTIME, &ts))
        return PTHREAD_QUEUE_ERROR;
    
    uint32_t sec = timeout / 1000;
    uint32_t nsec = (timeout - 1000 * sec) * 1000000;

    ts.tv_sec += sec;

    if (ts.tv_nsec + nsec > 1000000000)
        ts.tv_sec++;

    ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;

    /* Get queue lock */
    pthread_mutex_lock(&(queue->mutex));
    while (queue->items == queue->size) {
        ret = pthread_cond_timedwait(&(queue->cond_full), &(queue->mutex), &ts);
        if (ret != 0) {
            pthread_mutex_unlock(&(queue->mutex));
            return PTHREAD_QUEUE_FULL;
        }
    }

    /* Coby object from input buffer */
    memcpy(queue->buffer+(queue->in * queue->item_size), value, queue->item_size);
    queue->items++;
    queue->in = (queue->in + 1) % queue->size;
    pthread_mutex_unlock(&(queue->mutex));
    
    /* Nofify blocked threads */
    pthread_cond_broadcast(&(queue->cond_empty));
    
    return PTHREAD_QUEUE_OK;
    
}

int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, int timeout) {

    int ret;
    
    /* Calculate timeout */
    struct timespec ts;
    if (clock_gettime(CLOCK_REALTIME, &ts))
        return PTHREAD_QUEUE_ERROR;
    
    uint32_t sec = timeout / 1000;
    uint32_t nsec = (timeout - 1000 * sec) * 1000000;

    ts.tv_sec += sec;
    
    if (ts.tv_nsec + nsec > 1000000000)
        ts.tv_sec++;

    ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
    
    /* Get queue lock */
    pthread_mutex_lock(&(queue->mutex));
    while (queue->items == 0) {
        ret = pthread_cond_timedwait(&(queue->cond_empty), &(queue->mutex), &ts);
        if (ret != 0) {
            pthread_mutex_unlock(&(queue->mutex));
            return PTHREAD_QUEUE_EMPTY;
        }
    }

    /* Coby object to output buffer */
    memcpy(buf, queue->buffer+(queue->out * queue->item_size), queue->item_size);
    queue->items--;
    queue->out = (queue->out + 1) % queue->size;
    pthread_mutex_unlock(&(queue->mutex));
    
    /* Nofify blocked threads */
    pthread_cond_broadcast(&(queue->cond_full));

    return PTHREAD_QUEUE_OK;
    
}

int pthread_queue_items(pthread_queue_t * queue) {

    pthread_mutex_lock(&(queue->mutex));
    int items = queue->items;
    pthread_mutex_unlock(&(queue->mutex));
    
    return items;
    
}
